#include<stdio.h>
#include<stdlib.h>
#include<unistd.h>
#include<string.h>
#include<arpa/inet.h>
#include "/home/pi/Desktop/suntest/src/LOBOROBOT.c"

#include <stdio.h>
#include <stdlib.h>

#include <unistd.h>
#include <errno.h>
#include <string.h>
#include <netdb.h>
#include <sys/types.h>
#include <time.h>
#include <arpa/inet.h>
#include <wiringPi.h>
#include <softPwm.h>
#include<iostream>
#include"../include/Car.h"
using namespace std;
Car::Car()
{
	Motor_Init();
}
/*
void Car::init(int port)
{
		Motor_Init();
		int lfd=socket(AF_INET,SOCK_STREAM,0);
		if(lfd==-1)
		{
			cout<<"套接字创建失败"<<endl;
			return ;
		}
		int opt=1;
		setsockopt(lfd,SOL_SOCKET,SO_REUSEADDR,(const void *)&opt,sizeof(opt));
		struct sockaddr_in serv_addr;
		memset(&serv_addr,0,sizeof(serv_addr));
		serv_addr.sin_family=AF_INET;
		serv_addr.sin_port=htons(port);
		serv_addr.sin_addr.s_addr=htonl(INADDR_ANY);
		int ret=bind(lfd,(struct sockaddr*)&serv_addr,sizeof(serv_addr));
		if(ret==-1)
		{
			cout<<"绑定失败"<<endl;
			return;
		}
		ret=listen(lfd,64);
		if(ret==-1)
		{
			cout<<"监听失败"<<endl;
			return;
		}
		for(;;)
		{
			struct sockaddr_in client_addr;
			socklen_t client_len=sizeof(client_addr);
			int cfd=accept(lfd,(struct sockaddr*)&client_addr,&client_len);
			if(cfd==-1)
			{
				cout<<"连接失败"<<endl;
				continue;
			}
			else
			{
				cout<<"lianjie"<<endl;
			}
			for(;;)
			{
				char buf[1024]={0};
				int len=read(cfd,buf,sizeof(buf));
				if(len==-1)
				{
					cout<<"连接断开"<<endl;
					break;
				}
				if(len>0)
				{
					string data;
					data=buf;
					cout<<data<<endl;
					message_deal(data);
				}
				if(len==0)
				{
					cout<<"连接断开"<<endl;
					break;
				}
			}
			close(cfd);
		}
}
* */
void Car::message_deal(string data,int deliver)
{
	if(deal_map.count(data)==1)
	{
		cout<<"!!!"<<endl;
		(this->*deal_map[data])(deliver);
	}
}
void Car::forward(int speed)
{
	if(speed>=0&&speed<=100)
	{
		Motor_Run(MOTORA,FORWARD,speed);
		Motor_Run(MOTORB,FORWARD,speed);
		Motor_Run(MOTORC,FORWARD,speed);
		Motor_Run(MOTORD,FORWARD,speed);
	}
}
void Car::backward(int speed)
{
	if(speed>=0&&speed<=100)
	{
		Motor_Run(MOTORA,BACKWARD,speed);
		Motor_Run(MOTORB,BACKWARD,speed);
		Motor_Run(MOTORC,BACKWARD,speed);
		Motor_Run(MOTORD,BACKWARD,speed);
	}
}
void Car::left_turn(int speed)
{
	if(speed>=0&&speed<=100)
	{
		Motor_Run(MOTORA,BACKWARD,speed);
		Motor_Run(MOTORB,FORWARD,speed);
		Motor_Run(MOTORC,BACKWARD,speed);
		Motor_Run(MOTORD,FORWARD,speed);
	}
}
void Car::right_turn(int speed)
{
	if(speed>=0&&speed<=100)
	{
		Motor_Run(MOTORA,FORWARD,speed);
		Motor_Run(MOTORB,BACKWARD,speed);
		Motor_Run(MOTORC,FORWARD,speed);
		Motor_Run(MOTORD,BACKWARD,speed);
	}
}
void Car::left_forward(int speed)
{
	if(speed>=0&&speed<=100)
	{
		Motor_Stop(MOTORA);
		Motor_Run(MOTORB,FORWARD,speed);
		Motor_Run(MOTORC,FORWARD,speed);
		Motor_Stop(MOTORD);
	}
}
void Car::right_forward(int speed)
{
	if(speed>=0&&speed<=100)
	{
		Motor_Run(MOTORA,FORWARD,speed);
		Motor_Stop(MOTORB);
		Motor_Stop(MOTORC);
		Motor_Run(MOTORD,FORWARD,speed);
	}
	
}
void Car::left_back(int speed)
{
	if(speed>=0&&speed<=100)
	{
		Motor_Run(MOTORA,BACKWARD,speed);
		Motor_Stop(MOTORB);
		Motor_Stop(MOTORC);
		Motor_Run(MOTORD,BACKWARD,speed);
	}
}
void Car::right_back(int speed)
{
	if(speed>=0&&speed<=100)
	{
		Motor_Stop(MOTORA);
		Motor_Run(MOTORB,BACKWARD,speed);
		Motor_Run(MOTORC,BACKWARD,speed);
		Motor_Stop(MOTORD);
	}
}
void Car::big_arm(int angle)
{
	if(angle>=0&&angle<=120)
	set_servo_angle(14,angle);
}
void Car::small_arm(int angle)
{
	if(angle>=0&&angle<=120)
	set_servo_angle(12,angle);
}
void Car::plateform(int angle)
{
	if(angle>=0&&angle<=160)
	set_servo_angle(13,angle);
}
void Car::handle(int angle)
{
	if(angle>=10&&angle<=30)
	set_servo_angle(15,angle);
}
void Car::stop(int joke)
{
	Motor_Stop(MOTORA);
	Motor_Stop(MOTORB);
	Motor_Stop(MOTORC);
	Motor_Stop(MOTORD);
}
